/*
 * udsv.c
 *
 *  Created on: 2021年2月18日
 *      Author: taoyu
 */


#include "uds_types.h"
#include "udsv.h"
#include "udsv_hooks.h"
#include "udsv_events.h"
#include "udsv_settings.h"
#include "udsv_config.h"

typedef enum
{
	UDSV_XFER_STATE_INIT = 0,
	UDSV_XFER_STATE_READY,
	UDSV_XFER_STATE_BUSY,
	UDSV_XFER_STATE_ERROR,
}
UDSV_XFER_STATE;

typedef struct
{
	UDSV_XFER_STATE rxState;
	uint8 rxMsgData[UDSV_MESSAGE_BUFFER_SIZE];
	uint8 rxMsgLength;

	UDSV_XFER_STATE txState;
	uint8 txMsgData[UDSV_MESSAGE_BUFFER_SIZE];
	uint8 txMsgLength;

	// request analysis
	UDS_SID sid;			// service id
	uint8 rctp;				// routine control type
	uint16 did;				// data id
	uint16 rid;				// routine id

	// only for read/write memory by address
	uint8 addressAndLengthFormat;
	uint8 sizeOfMemoryAddress;
	uint8 sizeOfMemorySize;
	uint32 memoryAddress;
	uint32 memorySize;

	// only for IO Control
	UDS_IOCP ioControlParameter;
}
UDSV_DATA;

UDSV_DATA udsv_Data;

static void udsv_requestProcess(void);
static void udsv_sendResponse(UDS_NRC nrc);

static void udsv_readMemoryByAddress(void);
static void udsv_writeMemoryByAddress(void);
static void udsv_ioControl(void);
static void udsv_readDataByID(void);
static void udsv_writeDataByID(void);
static void udsv_routineControl(void);

void udsv_init(void)
{
	udsv_Data.rxState = UDSV_XFER_STATE_READY;
//	udsv_Data.txState = UDSV_XFER_STATE_READY;
}

void udsv_task(void)
{
	// when the state change to busy
	if(udsv_Data.rxState==UDSV_XFER_STATE_BUSY)
	{
		// process the received message
		udsv_requestProcess();
		// change the state back to ready, for the next request
		udsv_Data.rxState = UDSV_XFER_STATE_READY;
	}
}

// the message is received and passed in from the receiver
int udsvEvent_messageReceived(uint8 * pData, uint8 length)
{
	// check arguments
	if( !pData || !length || length>UDSV_MESSAGE_BUFFER_SIZE )
	{
		// error arguments
		return -1;
	}
	// when the state is NOT ready
	else if( udsv_Data.rxState != UDSV_XFER_STATE_READY)
	{
		// error state
		return -2;
	}
	// ok, the message is accepted
	else
	{
		uint8 i;
		// copy the data
		for(i=0;i<length;i++) udsv_Data.rxMsgData[i] = pData[i];
		// store the data length
		udsv_Data.rxMsgLength = length;
		// next state is the task to process the received message
		udsv_Data.rxState = UDSV_XFER_STATE_BUSY;
		// return the copied message length
		return udsv_Data.rxMsgLength;
	}
}

// process the received message
static void udsv_requestProcess(void)
{
	// get service ID
	udsv_Data.sid = (UDS_SID)udsv_Data.rxMsgData[0];

	// routine control type
	udsv_Data.rctp = (UDS_RCTP)udsv_Data.rxMsgData[1];

	// in many cases the next 2 bytes are data id
	udsv_Data.did = udsv_Data.rxMsgData[1];
	udsv_Data.did <<= 8;
	udsv_Data.did |= udsv_Data.rxMsgData[2];

	// routine control id
	udsv_Data.rid = udsv_Data.rxMsgData[2];
	udsv_Data.rid <<= 8;
	udsv_Data.rid |= udsv_Data.rxMsgData[3];

	// switch services
	switch( udsv_Data.sid )
	{
		/* io control by id */
		case	UDS_SID_IO_CONTROL:
			udsv_ioControl();
			break;

		/* read memory by address */
		case UDS_SID_READ_MEM_BY_ADDR:
			udsv_readMemoryByAddress();
			break;

		/* write memory by address */
		case UDS_SID_WRITE_MEM_BY_ADDR:
			udsv_writeMemoryByAddress();
			break;

		/* read data by ID */
		case	UDS_SID_READ_DATA_BY_ID:
			udsv_readDataByID();
			break;

		/* write data by ID */
		case	UDS_SID_WRITE_DATA_BY_ID:
			udsv_writeDataByID();
			break;

			/* routine control by id */
		case	UDS_SID_ROUTINE_CONTROL:
			udsv_routineControl();
			break;

		default:
			break;
	}
}

static void udsv_sendResponse(UDS_NRC nrc)
{
	if(nrc==UDS_NRC_OK)
	{
		udsvHook_sendMessage(udsv_Data.txMsgData, udsv_Data.txMsgLength);
	}
	else
	{
		udsv_Data.txMsgData[0] = 0x7F;
		udsv_Data.txMsgData[1] = (uint8)udsv_Data.sid;
		udsv_Data.txMsgData[2] = (uint8)nrc;
		udsv_Data.txMsgLength = 3;
		udsvHook_sendMessage(udsv_Data.txMsgData, udsv_Data.txMsgLength);
	}
}

// calculate memory address and size for read/write memory by address
static void udsv_calculateMemoryAddressAndSize(void)
{
	uint8 i;
	uint32 tmp;

	// memory address
	tmp = udsv_Data.rxMsgData[2];
	for(i=1;i<udsv_Data.sizeOfMemoryAddress;i++)
	{
		tmp <<= 8;
		tmp |= udsv_Data.rxMsgData[2+i];
	}
	udsv_Data.memoryAddress = tmp;

	// memory size
	tmp = udsv_Data.rxMsgData[2+udsv_Data.sizeOfMemoryAddress];
	for(i=1;i<udsv_Data.sizeOfMemorySize;i++)
	{
		tmp <<= 8;
		tmp |= udsv_Data.rxMsgData[2+udsv_Data.sizeOfMemoryAddress+i];
	}
	udsv_Data.memorySize = tmp;
}

static void udsv_readMemoryByAddress(void)
{
	// request format: RMBA(23) ALFID MA MS

	// process address and length
	udsv_Data.addressAndLengthFormat = udsv_Data.rxMsgData[1];
	udsv_Data.sizeOfMemoryAddress = udsv_Data.addressAndLengthFormat & 0x0F;
	udsv_Data.sizeOfMemorySize = udsv_Data.addressAndLengthFormat >> 4;

	// check request length, at least there should be 4 bytes: 23 ALFID MA MS
	if( udsv_Data.rxMsgLength<4 )
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check request length again, the size of address or size should not be 0
	else if( !udsv_Data.sizeOfMemoryAddress || !udsv_Data.sizeOfMemorySize)
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check request length again, the format should match with the address data
	else if( udsv_Data.rxMsgLength!= 2+udsv_Data.sizeOfMemoryAddress+udsv_Data.sizeOfMemorySize)
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check the address and length plausibility
	else
	{
		// calculate the requested memory address and size
		udsv_calculateMemoryAddressAndSize();

		// check plausibility MIN
		if( udsv_Data.memoryAddress < UDSV_MEMORY_ADDRESS_MIN )
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// check plausibility MAX
		else if( udsv_Data.memoryAddress+udsv_Data.memorySize > UDSV_MEMORY_ADDRESS_MAX )
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// address and length plausibility pass
		else
		{
			// response data length
			udsv_Data.txMsgLength = 1+udsv_Data.memorySize;

			// check buffer availability, response format : SID Data ...
			if(udsv_Data.txMsgLength <= sizeof(udsv_Data.txMsgData))
			{
				uint8 i;
				uint8 * pSrc = (uint8*) udsv_Data.memoryAddress;
				uint8 * pDst = &udsv_Data.txMsgData[1];
				for(i=0;i<udsv_Data.memorySize;i++) pDst[i] = pSrc[i];
				udsv_Data.txMsgData[0] = 0x40+UDS_SID_READ_MEM_BY_ADDR;
				udsv_sendResponse(UDS_NRC_OK);
			}
			// buffer not available
			else
			{
				udsv_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
			}
		}
	}
}

static void udsv_writeMemoryByAddress(void)
{
	// request format: WMBA(3D) ALFID MA MS Data ...

	// process address and length
	udsv_Data.addressAndLengthFormat = udsv_Data.rxMsgData[1];
	udsv_Data.sizeOfMemoryAddress = udsv_Data.addressAndLengthFormat & 0x0F;
	udsv_Data.sizeOfMemorySize = udsv_Data.addressAndLengthFormat >> 4;

	// check request length, at least there should be 5 bytes
	if( udsv_Data.rxMsgLength<5 )
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check request length again, the size of address or size should not be 0
	else if( !udsv_Data.sizeOfMemoryAddress || !udsv_Data.sizeOfMemorySize)
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	// check the address and length plausibility
	else
	{
		// calculate the requested memory address and size
		udsv_calculateMemoryAddressAndSize();

		// check request length again, the format should match with the address/size data
		if( udsv_Data.rxMsgLength!= 2+udsv_Data.sizeOfMemoryAddress+udsv_Data.sizeOfMemorySize+udsv_Data.memorySize)
		{
			udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
		}
		// check plausibility MIN
		else if( udsv_Data.memoryAddress < UDSV_MEMORY_ADDRESS_MIN )
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// check plausibility MAX
		else if( udsv_Data.memoryAddress+udsv_Data.memorySize > UDSV_MEMORY_ADDRESS_MAX )
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// address and length plausibility pass
		else
		{
			// write the data to the address
			uint8 i;
			uint8 * pDst = (uint8*) udsv_Data.memoryAddress;
			uint8 * pSrc = &udsv_Data.rxMsgData[2+udsv_Data.sizeOfMemoryAddress+udsv_Data.sizeOfMemorySize];
			for(i=0;i<udsv_Data.memorySize;i++) pDst[i] = pSrc[i];

			// prepare response message: SID(7D) ALFID MA MS
			udsv_Data.txMsgLength = 2+udsv_Data.sizeOfMemoryAddress+udsv_Data.sizeOfMemorySize;
			udsv_Data.txMsgData[0] = 0x40+UDS_SID_WRITE_MEM_BY_ADDR;
			for(i=1;i<udsv_Data.txMsgLength;i++) udsv_Data.txMsgData[i] = udsv_Data.rxMsgData[i];
			udsv_sendResponse(UDS_NRC_OK);
		}
	}
}

static void udsv_ioControl(void)
{
	// request format: IOCBI(2F) DIDH DIDL M1 C1 ... C2 ...

	// store the io control parameter (M1)
	udsv_Data.ioControlParameter = (UDS_IOCP)udsv_Data.rxMsgData[3];

	// check request length, at least 4 bytes: 2F, DID, M1
	if( udsv_Data.rxMsgLength<4 )
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	else
	{
		uint8 i;

		// search for the did
		for(i=0;i<UDSV_IOC_DESC_NUMBER;i++)
		{
			// match the did
			if( udsv_iocDesc[i].did==udsv_Data.did)
			{
				break;
			}
			else
			{
				// try next did
			}
		}

		// no did found
		if(i>=UDSV_IOC_DESC_NUMBER)
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// did found, check function
		else if( !udsv_iocDesc[i].ioControl )
		{
			// function is null
			udsv_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
		}
		// function is ok
		else
		{
			// call the interface
			UDS_NRC nrc = udsv_iocDesc[i].ioControl(udsv_Data.did, udsv_Data.ioControlParameter, &udsv_Data.rxMsgData[4], udsv_Data.rxMsgLength-4);
			// prepare positive response
			if( nrc==UDS_NRC_OK )
			{
				for(i=0;i<udsv_Data.rxMsgLength;i++) udsv_Data.txMsgData[i] = udsv_Data.rxMsgData[i];
				udsv_Data.txMsgLength = udsv_Data.rxMsgLength;
				udsv_Data.txMsgData[0] = 0x40 + UDS_SID_IO_CONTROL;
			}
			// send response
			udsv_sendResponse(nrc);
		}
	}
}

static void udsv_readDataByID(void)
{
	// request format: RDBI(22) DIDH DIDL

	// check length
	if(udsv_Data.rxMsgLength!=3)
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	else
	{
		uint8 i;

		// search for the did
		for(i=0;i<UDSV_RDBI_DESC_NUMBER;i++)
		{
			// match the did
			if( udsv_rdbiDesc[i].did==udsv_Data.did)
			{
				break;
			}
			else
			{
				// try next did
			}
		}

		// no did found
		if(i>=UDSV_RDBI_DESC_NUMBER)
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// did found, check function
		else if( !udsv_rdbiDesc[i].readDataByID )
		{
			// function is null
			udsv_sendResponse(UDS_NRC_CONDITION_NOT_CORRECT);
		}
		// function is ok
		else
		{
			uint8 dataLength;
			// call the interface
			UDS_NRC nrc = udsv_rdbiDesc[i].readDataByID(udsv_Data.did, &udsv_Data.txMsgData[3], sizeof(udsv_Data.txMsgData)-3, &dataLength);
			// prepare positive response
			if( nrc==UDS_NRC_OK )
			{
				// response format: RDBI(62) DIDH DIDL Data ...
				udsv_Data.txMsgData[0] = 0x40 + UDS_SID_READ_DATA_BY_ID;
				udsv_Data.txMsgData[1] = udsv_Data.rxMsgData[1];
				udsv_Data.txMsgData[2] = udsv_Data.rxMsgData[2];
				udsv_Data.txMsgLength = 3+dataLength;
			}
			// send response
			udsv_sendResponse(nrc);
		}
	}
}

static void udsv_writeDataByID(void)
{

}

static void udsv_routineControl(void)
{
	// request format: RC(31) RCTP RIDH RIDL (Data ...)

	// check length
	if(udsv_Data.rxMsgLength<4)
	{
		udsv_sendResponse(UDS_NRC_INCORRECT_MESSAGE_LENGTH_OR_INVALID_FORMAT);
	}
	else
	{
		uint8 i;

		// positive response have same heading
		for(i=0;i<4;i++) udsv_Data.txMsgData[i] = udsv_Data.rxMsgData[i];
		udsv_Data.txMsgLength = 4;

		// search for the routine id
		for(i=0;i<UDSV_RC_DESC_NUMBER;i++)
		{
			// match the routine id
			if( udsv_rcDesc[i].rid==udsv_Data.rid)
			{
				break;
			}
			else
			{
				// try next did
			}
		}

		// no routine id found
		if(i>=UDSV_RC_DESC_NUMBER)
		{
			udsv_sendResponse(UDS_NRC_REQUEST_OUT_OF_RANGE);
		}
		// routine id found, check sub-functions
		else
		{
			// run sub function
			switch (udsv_Data.rctp)
			{
				case	UDS_RCTP_RESERVED_0:	// reserved
					// function sub function is not supported
					udsv_sendResponse(UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED);
					break;
				case	UDS_RCTP_START:	// start
					break;
				case	UDS_RCTP_STOP:	// stop
					break;
				case	UDS_RCTP_REQUEST_RESULT:	// request result
					break;
				default:	// reserved
					// function sub function is not supported
					udsv_sendResponse(UDS_NRC_SUB_FUNCTION_NOT_SUPPORTED);
					break;
			}
		}
	}
}

